{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "07e30b5a",
   "metadata": {},
   "outputs": [],
   "source": [
    "from stretch_body.robot import Robot\n",
    "import math\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "825d1826",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Robot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "67b31905",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.startup()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "6169ea99",
   "metadata": {},
   "outputs": [],
   "source": [
    "if not robot.is_calibrated():\n",
    "    robot.home()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "d09af09c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# robot.stow()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "129df87c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# robot.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "98317fa1",
   "metadata": {},
   "source": [
    "# Camera Transforms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "32c5336a",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import os\n",
    "urdf_path = os.path.join(os.getenv(\"HELLO_FLEET_PATH\"), os.getenv(\"HELLO_FLEET_ID\"), \"exported_urdf\", \"stretch.urdf\")\n",
    "mesh_path = os.path.join(os.getenv(\"HELLO_FLEET_PATH\"), os.getenv(\"HELLO_FLEET_ID\"), \"exported_urdf\")\n",
    "\n",
    "\n",
    "from pytransform3d.urdf import UrdfTransformManager\n",
    "import pytransform3d.transformations as pt\n",
    "import pytransform3d.visualizer as pv\n",
    "import numpy as np\n",
    "\n",
    "\n",
    "tm = UrdfTransformManager()\n",
    "with open(urdf_path, \"r\") as f:\n",
    "    urdf = f.read()\n",
    "    tm.load_urdf(urdf, mesh_path=mesh_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "71f75d17",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "head_pan: 1.569262346007011 head_tilt -0.01227184630308513\n",
      "[[ 0.00646493  0.99944347 -0.03272549 -0.01340795]\n",
      " [-0.01678736  0.03283003  0.99931996 -1.31305613]\n",
      " [ 0.99983818 -0.00591116  0.01699026 -0.00277494]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "s = robot.get_status()\n",
    "head_pan = s['head']['head_pan']['pos']\n",
    "head_tilt = s['head']['head_tilt']['pos']\n",
    "print('head_pan:', head_pan, 'head_tilt', head_tilt)\n",
    "\n",
    "\n",
    "# Get Camera transform\n",
    "tm.set_joint(\"joint_head_pan\", head_pan)\n",
    "tm.set_joint(\"joint_head_pan\", head_pan)\n",
    "camera_transform = tm.get_transform('base_link', 'camera_link')\n",
    "\n",
    "print(camera_transform)\n",
    "\n",
    "# use this transform in du.transform_pose (i.e. see get_current_pcd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b4478898",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize URDF using Open3D\n",
    "fig = pv.figure()\n",
    "fig.plot_graph(tm, \"base_link\", s=0.1, show_visuals=True)\n",
    "fig.view_init()\n",
    "fig.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8836f17d",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7c833350",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fead703c",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "389e7895",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
